#include "dwarf2_panorama.h"

#include "motor_controller.h"

#include "dwarf_utils.h"
#include "elog.h"
#include "elog_file.h"
#include "ws_constant.h"
#include "album.h"
#include "album_common.h"
#include "ws_response.h"
#include "ws_constant.h"
#include "params_setting.h"

#include <mutex>

using namespace std;
namespace fs = std::filesystem;

static int cur_photo_count = 0;
static int cur_rows = 3;
static int cur_cols = 3;

static std::queue<std::function<void(MEDIA_BUFFER)>> _img_recive_acts; 
static mutex panorama_mutex;

Dwarf2Panorama::Dwarf2Panorama(StepMotorUser* yaw_moto, StepMotorUser* pitch_moto, CamTele* cam) {
	_yaw_moto = yaw_moto;
	_pitch_moto = pitch_moto;
	_cam = cam;
	m_album_ = Album();
}

int Dwarf2Panorama::notifiyPhotoProgress() {
	wsResNotifyPanoramaProgress(CMD_NOTIFY_PANORAMA_PROGRESS, MODULE_PANORAMA, CMD_TYPE_NOTIFICATION, cur_rows * cur_cols, cur_photo_count);
	return 0;
}

int Dwarf2Panorama::CapturePanoramaImages(std::string save_path, int rows, int cols, PanoramaInfoBase& out) {
	log_i("CapturePanormaImages begin");
	int ret;
	
	unique_lock<mutex> lock(panorama_mutex);

	rgb_power_.setRgbEffect(RGB_EFFECT_PANORAMA);// 强制全景灯效，因为复位前还没设置状态

	if(!fs::exists(save_path)){
		try {
            fs::create_directory(save_path);
        } catch (const std::filesystem::filesystem_error& e) {
            log_i("std::filesystem::create_directory:%s", e.what());
			ret = -WS_SDCARD_WRITE_ERROR;
			return ret;
		}
	}

	if (_cam->isCameraOpen() < 0) {
		log_i("camera is not open");
        return -CODE_CAMERA_TELE_CLOSED;
    }

	if(CamTele::getPhotoState() != CamTelePhotoState::CAM_TELE_PHOTO_IDLE){
		log_i("camera photo state not idel");
		ret= -CODE_CAMERA_TELE_WORKING_BUSY;
		return ret;
	}

	int res;

	cur_photo_count = 0;

	double cur_yaw = 0;
	double cur_pitch = 0;

	wsResNotifyCamFunctionState(CMD_NOTIFY_TELE_FUNCTION_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, CAM_FUNCTION_PANORAMA, STATE_RUNNING);
	//首先复位，获得相对位置走了多少，从而知道初始位置
	ret = _yaw_moto->resetAndSetParameterInDegree(YAW_LEFT, cols * YawPerStep / 2, YAW_LEFT, STEP_MOTOR_START_RESET_POSITION);
    if (ret < 0) {
        log_i("yaw moto ressetInDegree failed");
		ret = -CODE_PANORAMA_MOTOR_RESET_FAILED;
        SetCurrentState(PanoramaState::IDEL);
        return ret;
    }

    _yaw_moto->run();

    ret = _pitch_moto->resetAndSetParameterInDegree(PITCH_DOWN, rows * PitchPerStep / 2, PITCH_UP, STEP_MOTOR_START_RESET_POSITION);
    if (ret < 0) {
        log_i("pitch moto ressetInDegree failed");
		ret = -CODE_PANORAMA_MOTOR_RESET_FAILED;
        SetCurrentState(PanoramaState::IDEL);
        return ret;
    }

    _pitch_moto->run();

	_yaw_moto->waitMotorToStop();
    _pitch_moto->waitMotorToStop();

	log_i("rows = %d, cols = %d", rows, cols);
	SetCurrentState(PanoramaState::Running);

	if(GetCurrentState() == PanoramaState::IDEL){
		log_i("panorama stop by user");
		SetCurrentState(PanoramaState::IDEL);
		return 0;
	}
	log_i("start panorama photogragh");

	std::this_thread::sleep_for(std::chrono::seconds(5));
	
	lock.unlock();
	vector<cv::Mat> thimgs(rows * cols);
	for (int row = 0; row < rows; row++)
	{
		if(row != 0){
			res = _pitch_moto->setParameterInDegree(PitchPerStep,PitchSpeed,PitchResolutionLevel,PitchRampPlus,MotorPitchDirection::PITCH_DOWN);
			cur_pitch -= PitchPerStep;
			if(res != 0){
				log_i("pitch moto setParameterInDegree failed");
				ret = -WS_INVAID_PARAM;
				SetCurrentState(PanoramaState::IDEL);
				return ret;
			}
			_pitch_moto->run();
			res = _pitch_moto->waitMotorToStop();
			if (res == -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED) {
				log_i("pitch moto hit limit");
				ret = -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED;
				SetCurrentState(PanoramaState::IDEL);
				return ret;
			}
		}

		for (int col = (row % 2) ? cols - 1 : 0 ; (row % 2) ? col >= 0 : col < cols; (row % 2) ? col-- : col++)
		{	
			if(GetCurrentState() == PanoramaState::Stopping){
				log_i("panorama stop by user");
				SetCurrentState(PanoramaState::IDEL);
				return 0;
			}
			if(col != (row % 2 ? cols - 1 : 0)){
				res = _yaw_moto->setParameterInDegree(YawPerStep,YawSpeed,YawResolutionLevel,YawRampPlus,row % 2 == 0 ? MotorYawDirection::YAW_RIGHT : MotorYawDirection::YAW_LEFT );
				cur_yaw += row % 2 == 0 ? YawPerStep : -YawPerStep;
				if(res != 0){
					log_i("yaw moto setParameterInDegree failed");
					ret = -WS_INVAID_PARAM;
					SetCurrentState(PanoramaState::IDEL);
					return ret;
				}

				_yaw_moto->run();
				res = _yaw_moto->waitMotorToStop();
				if (res == -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED) {
					log_i("yaw moto hit limit");
					ret = -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED;
					SetCurrentState(PanoramaState::IDEL);
					return ret;
				}
			}
			std::this_thread::sleep_for(std::chrono::seconds(1));
			double current_exp;
			_cam->getExp(&current_exp);
			std::this_thread::sleep_for(std::chrono::milliseconds((int)(current_exp * 1000)));
			
			//开始拍摄
			fs::path file_save_path = fs::path(save_path) / (ToSpecifyDigit(row,2) + "_" + ToSpecifyDigit(col,2) + ".jpg");
			int idx = row * cols + col;
			log_i(("index " + to_string(idx) +  " start").c_str());
			void* nv12_buffer;
			MEDIA_BUFFER nv12_mediabuffer;
			nv12_mediabuffer = _cam->getNv12MediaBufferByPass();
			if(!nv12_mediabuffer){
				log_i("get nv12_mediabuffer failed");
				ret = -CODE_PANORAMA_PHOTO_FAILED;
				SetCurrentState(PanoramaState::IDEL);
				return ret;
			}
			nv12_buffer = RK_MPI_MB_GetPtr(nv12_mediabuffer);
			log_i("get media buffer success");
			if(!nv12_buffer){
				log_i("get nv12 image failed");
				ret = -CODE_PANORAMA_PHOTO_FAILED;
				SetCurrentState(PanoramaState::IDEL);
				return ret;
			}

			cv::Mat nv12img,bgrimg;
			if(_cam->getBinningState()){
				nv12img = cv::Mat(1620,1920,CV_8UC1,nv12_buffer);
			}else{
				nv12img = cv::Mat(3240,3840,CV_8UC1,nv12_buffer);
			}
			cv::cvtColor(nv12img,bgrimg,cv::COLOR_YUV2BGR_NV12);
			// cv::resize(bgrimg,thimgs[idx],cv::Size(64,36));
			if (cv::imwrite(file_save_path.string(),bgrimg) == false) {
				log_i("image save failed");
				ret = -WS_SDCARD_WRITE_ERROR;
				SetCurrentState(PanoramaState::IDEL);
				return ret;
			}
			cur_photo_count++;
			wsResNotifyPanoramaProgress(CMD_NOTIFY_PANORAMA_PROGRESS, MODULE_PANORAMA, CMD_TYPE_NOTIFICATION, rows * cols, cur_photo_count);
			rkfree(nv12_mediabuffer);
		}
	}
	
	//简单拼成缩略图
	// fs::path thumbnail_path = fs::path(save_path) / "thumbnail.jpg";
	// cv::Mat thum(rows * 36,cols * 64,CV_8UC3);
	// for (int i = 0; i < thimgs.size(); i++)
	// {
	// 	int row = i / cols;
	// 	int col = i % cols;
	// 	cv::Mat tarm(thum,cv::Rect(col * 64,row * 36,64,36));
	// 	thimgs[i].copyTo(tarm);
	// }
	// cv::imwrite(thumbnail_path,thum);
	
	out.rows = rows;
	out.cols = cols;
	out.yaw_range = cols * YawPerStep;
	out.pitch_range = rows * PitchPerStep;

	cout << "cur_yaw is " << cur_yaw << endl;
	cout << "cur_pitch is " << cur_pitch << endl;

	cur_yaw = cur_yaw - (cols * YawPerStep) / 2;
	cur_pitch = cur_pitch + (rows * PitchPerStep) / 2;

	SetCurrentState(PanoramaState::IDEL);
	
	{//回到原来的位置，复位
		res = _yaw_moto->setParameterInDegree(abs(cur_yaw),YawSpeed,YawResolutionLevel,YawRampPlus,cur_yaw > 0 ? MotorYawDirection::YAW_LEFT : MotorYawDirection::YAW_RIGHT);
		if(res != 0){
			log_i("yaw moto setParameterInDegree failed");
			ret = -WS_INVAID_PARAM;
			SetCurrentState(PanoramaState::IDEL);
			return ret;
		}
		_yaw_moto->run();
		_yaw_moto->waitMotorToStop();
		res = _pitch_moto->setParameterInDegree(abs(cur_pitch),PitchSpeed,PitchResolutionLevel,PitchRampPlus, cur_pitch > 0 ? MotorPitchDirection::PITCH_DOWN : MotorPitchDirection::PITCH_UP);
		if(res != 0){
			log_i("pitch moto setParameterInDegree failed");
			ret = -WS_INVAID_PARAM;
			SetCurrentState(PanoramaState::IDEL);
			return ret;
		}

		_pitch_moto->run();
		_pitch_moto->waitMotorToStop();
	}

	return 0;
}

int Dwarf2Panorama::StopPanoramaCapture(){
	lock_guard<mutex> lock(panorama_mutex);
	SetCurrentState(PanoramaState::Stopping);
	return 0;
}

int Dwarf2Panorama::handleMessage(WsPacket& ws_packet){
	uint cmd = ws_packet.cmd();
	string data = ws_packet.data();
	log_i("handle panorama message, cmd = %d", ws_packet.cmd());
	int ret;

	if(CMD_PANORAMA_START_GRID == cmd){
		ReqStartPanoramaByGrid req;

		if (checkSdExist() != 0) {
			log_i("Sdcard not exist");
			ret = -WS_SDCARD_NOT_EXIST;
			wsCommonResponse(CMD_PANORAMA_START_GRID, MODULE_PANORAMA, CMD_TYPE_RESPONSE, ret);
			return ret;
		}

		if(req.ParseFromString(data)){
			filesystem::path dir_path = filesystem::path(album_path) / panorama_path;
			if(!filesystem::exists(dir_path)){
				filesystem::create_directory(dir_path);
			}
			string panorama_name = "DWARF_PANORAMA_" + getTimeStamp();
			filesystem::path fpath = dir_path / panorama_name;
			cur_rows = getPanoramaRow();
			cur_cols = getPanoramaCol();
			log_i("cur_rows = %d, cur_cols = %d", cur_rows, cur_cols);
			float yaw_range = cur_cols * YawPerStep;
			float pitch_range = cur_rows * PitchPerStep;

			opMode_t exp_mode;
			double exp;
			int gain;
			_cam->getExpMode(&exp_mode);
			if(exp_mode == opMode_t::OP_AUTO){
				_cam->getExp(&exp);
				_cam->getGain(&gain);
				_cam->setExp(exp);
				_cam->setGain(gain);
			}
			
			opMode_t wb_mode;
			_cam->getWBMode(&wb_mode);
			if(wb_mode == opMode_t::OP_AUTO){
				_cam->setWBMode(opMode_t::OP_INVAL);			
			}

			PanoramaInfoBase info;
			ret = CapturePanoramaImages(fpath.string(),cur_rows,cur_cols,info);
			log_i("panorama completed, ret = %d", ret);

			if (ret < 0)
				rgb_power_.setRgbEffect(RGB_EFFECT_FAIL);
			else 
				rgb_power_.setRgbEffect(RGB_EFFECT_SUCCESS);

			if(exp_mode == opMode_t::OP_AUTO){
				_cam->setGain(gain);
				usleep(300 * 1000);
				_cam->setExpMode(opMode_t::OP_AUTO);
			}

			if(wb_mode == opMode_t::OP_AUTO){
				_cam->setWBMode(opMode_t::OP_AUTO);	
			}

			m_album_.album_pano_.updatePano(fpath.string(), cur_rows, cur_cols);
		
			wsResNotifyCamFunctionState(CMD_NOTIFY_TELE_FUNCTION_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, CAM_FUNCTION_PANORAMA, STATE_IDLE);
			wsCommonResponse(CMD_PANORAMA_START_GRID, MODULE_PANORAMA, CMD_TYPE_RESPONSE, ret);
			
		}else{
			log_i("Parameter convert failed");
			ret = -WS_PARSE_PROTOBUF_ERROR;
			wsCommonResponse(CMD_PANORAMA_START_GRID, MODULE_PANORAMA, CMD_TYPE_RESPONSE, ret);
			return ret;
		}
	}else if(CMD_PANORAMA_START_EULER == cmd){
		// ReqStartPanoramaByEulerRange req;
		// if(req.ParseFromString(data)){
		// 	filesystem::path dir_path = filesystem::path(album_path) / panorama_path;
		// 	if(!filesystem::exists(dir_path)){
		// 		filesystem::create_directory(dir_path);
		// 	}
		// 	string panorama_name = "DWARF_PANORAMA_" + getTimeStamp();
		// 	filesystem::path fpath = dir_path / panorama_name;
		// 	PanoramaInfoBase info;
		// 	CapturePanoramaImages(fpath.string(),req.yaw_range(),req.pitch_range(),info);
		// }else{
		// 	log_i("Parameter convert failed");
		// 	ret = -WS_PARSE_PROTOBUF_ERROR;
		// 	wsCommonResponse(CMD_PANORAMA_START_EULER, MODULE_PANORAMA, CMD_TYPE_RESPONSE, ret);
		// 	return ret;
		// }

	}else if(CMD_PANORAMA_STOP == cmd){
		ReqStopPanorama req;
		if(req.ParseFromString(data)){
			StopPanoramaCapture();
			wsCommonResponse(CMD_PANORAMA_STOP, MODULE_PANORAMA, CMD_TYPE_RESPONSE, 0);
		}
	}

	return 0;
}